#include "State.h"
#include "Logger.h"
#include <algorithm>
#include <stdlib.h>
#include <iomanip>
using namespace std;

//constructor
State::State()
{
    gameover = 0;
    turn = 0;
};

//deconstructor
State::~State()
{
};

//sets the state up
void State::setup()
{
    grid = vector<vector<Square> >(rows, vector<Square>(cols, Square()));
    visitedHistory = vector< vector<int> > (rows, vector<int>(cols,0));
    visibleGrid = vector< vector<bool>  > (rows, vector<bool>(cols,false));
};

//resets all non-wall squares to land and clears the bots ant vector
void State::reset()
{
    ants.clear();
    enemies.clear();
    food.clear();
    myHills.clear();
    enemyHills.clear();
    foodTrackingPathLengths.clear();
    frontLine.clear();

    while(!playerQ.empty())
	playerQ.pop();
    for(int row=0; row<rows; row++)
	for(int col=0; col<cols; col++) {
            if(!grid[row][col].isWater)
                grid[row][col].reset();
	    visibleGrid[row][col] = false;
	}
};

//outputs move information to the engine
void State::makeMove(const Location &loc, int direction)
{
    cout << "o " << loc.row << " " << loc.col << " " << CDIRECTIONS[direction] << endl;
    Logger::theLogger() << "Issuing order : " << loc.row << "," << loc.col << "," << CDIRECTIONS[direction] << endl;
    Location nLoc = getLocation(loc, direction);
    grid[nLoc.row][nLoc.col].ant = grid[loc.row][loc.col].ant;
    grid[loc.row][loc.col].ant = -1;
    antMoveHistory[nLoc] = antMoveHistory[loc];
    antMoveHistory[nLoc][loc] = turn;
    antMoveHistory.erase(loc);
    visitedHistory[nLoc.row][nLoc.col] +=1;
    increaseVision(nLoc);
};

int State::mockMakeMove(const Location& loc,int direction) {
    Location nLoc = getLocation(loc, direction);
    int saveAnt = grid[nLoc.row][nLoc.col].ant;
    grid[nLoc.row][nLoc.col].ant = grid[loc.row][loc.col].ant;
    grid[loc.row][loc.col].ant = -1;
    return saveAnt;
}

void State::unmakeMockMove(const Location &loc,int direction,int savedAnt) {
    Location nLoc = getLocation(loc, direction);
    grid[loc.row][loc.col].ant = grid[nLoc.row][nLoc.col].ant;
    grid[nLoc.row][nLoc.col].ant = savedAnt;
}

//returns the euclidean distance between two locations with the edges wrapped
double State::distance(const Location &loc1, const Location &loc2) const
{
    int d1 = abs(loc1.row-loc2.row),
        d2 = abs(loc1.col-loc2.col),
        dr = min(d1, rows-d1),
        dc = min(d2, cols-d2);
    return sqrt(dr*dr + dc*dc);
};

//returns the manhattan distance between two locations with the edges wrapped
double State::manhattan_distance(const Location &loc1, const Location &loc2) const
{
   int d1 = abs(loc1.row-loc2.row),
       d2 = abs(loc1.col-loc2.col),
       dr = min(d1, rows-d1),
       dc = min(d2, cols-d2);
   return dr+dc;
}

//returns the new location from moving in a given direction with the edges wrapped
Location State::getLocation(const Location &loc, int direction) const
{
    return Location( (loc.row + DIRECTIONS[direction][0] + rows) % rows,
                     (loc.col + DIRECTIONS[direction][1] + cols) % cols );
};

/*
    This function will update update the lastSeen value for any squares currently
    visible by one of your live ants.

    BE VERY CAREFUL IF YOU ARE GOING TO TRY AND MAKE THIS FUNCTION MORE EFFICIENT,
    THE OBVIOUS WAY OF TRYING TO IMPROVE IT BREAKS USING THE EUCLIDEAN METRIC, FOR
    A CORRECT MORE EFFICIENT IMPLEMENTATION, TAKE A LOOK AT THE GET_VISION FUNCTION
    IN ANTS.PY ON THE CONTESTS GITHUB PAGE.
*/
void State::updateVisionInformation()
{
    std::queue<Location> locQueue;
    Location sLoc, cLoc, nLoc;
    visiblelocations=0;
    for(int a=0; a<(int) ants.size(); a++)
    {
        sLoc = ants[a];
        locQueue.push(sLoc);

        std::vector<std::vector<bool> > visited(rows, std::vector<bool>(cols, 0));

        while(!locQueue.empty())
        {
            cLoc = locQueue.front();
            locQueue.pop();

            for(int d=0; d<TDIRECTIONS; d++)
            {
                nLoc = getLocation(cLoc, d);

                if(!visited[nLoc.row][nLoc.col] && distance(sLoc, nLoc) <= viewradius)
                {
                    grid[nLoc.row][nLoc.col].lastSeen = turn;
		    visiblelocations++; //keep track of # number of locations that we can see
		    visibleGrid[nLoc.row][nLoc.col] = true;
                    locQueue.push(nLoc);
                }
                visited[nLoc.row][nLoc.col] = 1;
            }
        }
    }
};

/** Checks if an ant at a new location will increase vision. 
  *
  */
int  State::increasesVision(const Location& loc) const  {
    std::queue<Location> locQueue;
    int newSquares = 0;
    locQueue.push(loc);
    std::vector<std::vector<bool> > visited(rows,std::vector<bool>(cols,0));
    while(!locQueue.empty()) {
	Location cLoc = locQueue.front();
	locQueue.pop();
	for(int d=0;d<TDIRECTIONS;d++) {
	    Location nLoc = getLocation(cLoc,d);
	    if (!visited[nLoc.row][nLoc.col] && distance(loc,nLoc) <=viewradius) {
		if (visibleGrid[nLoc.row][nLoc.col]!=true) {
		    //visibleGrid[nLoc.row][nLoc.col] = true;
		    newSquares++;
		}
		locQueue.push(nLoc);
	    }
	    visited[nLoc.row][nLoc.col] = 1;
	}
    }
    return newSquares;
}

/** Modifies the visibleGrid so that subsequent queries to increaseVision are correct
 *  Called by makeMove and should not be called by end users normally.
 *
 */
int State::increaseVision(const Location& loc) {
    std::queue<Location> locQueue;
    int newSquares = 0;
    locQueue.push(loc);
    std::vector<std::vector<bool> > visited(rows,std::vector<bool>(cols,0));
    while(!locQueue.empty()) {
	Location cLoc = locQueue.front();
	locQueue.pop();
	for(int d=0;d<TDIRECTIONS;d++) {
	    Location nLoc = getLocation(cLoc,d);
	    if (!visited[nLoc.row][nLoc.col] && distance(loc,nLoc) <=viewradius) {
		if (visibleGrid[nLoc.row][nLoc.col]!=true) {
		    visibleGrid[nLoc.row][nLoc.col] = true;
		    newSquares++;
		}
		locQueue.push(nLoc);
	    }
	    visited[nLoc.row][nLoc.col] = 1;
	}
    }
    return newSquares;
}

//AnalyzeMap will update some pathfinding information that will be useful
//when finding optimal paths from locA to locB.
//This function will be called every turn to update our knowledge of map and
//improve pathfinding.
void State::analyzeMap() {
    Timer t;
    t.start();
    map<int,int> playerAnts;
    for(int i=0;i<enemies.size();i++) {
	playerAnts[grid[enemies[i].row][enemies[i].col].ant] +=1;
    }
    for(map<int,int>::iterator it = playerAnts.begin();it!=playerAnts.end();it++) {
	Logger::theLogger() << "player_id:" << it->first << ",numAnts:" <<it->second <<endl;
	playerQ.push(PlayerQueueEntry(it->first,it->second));
    }

    frontLine.clear();
    //Now update antDestinationMaps
    //remove any ants that were destroyed.
    for(map<Location,Location>::iterator it = antDestinationMap.begin();
	it!=antDestinationMap.end();it++) {
	if (find(ants.begin(),ants.end(),it->first)==ants.end()) 
	    antDestinationMap.erase(it);
    }
    //Now update antWalkMap
    //remove any ants that were destroyed.
    for(map<Location,AntWalkDetails>::iterator it = antWalkMap.begin();
	it!=antWalkMap.end();it++) {
	if (find(ants.begin(),ants.end(),it->first)==ants.end()) 
	    antWalkMap.erase(it);
    }

    Logger::theLogger() << "Starting foodHistory update at:" << t.getTime() << "ms" << endl;
    Logger::theLogger() << "Erasing invalid food history " << endl;
    for(map<Location,int>::iterator it=foodHistory.begin();it!=foodHistory.end();
	it++) {
	Location foodLoc = it->first;
	//remove any food that we knew about but is not there anymore
	if ((grid[foodLoc.row][foodLoc.col].lastSeen==turn) &&
	    (!grid[foodLoc.row][foodLoc.col].isFood))
	    foodHistory.erase(it);
    }
    Logger::theLogger() << "Finished foodHistory update at:" << t.getTime() << "ms" << endl;

    Logger::theLogger() << "Starting enemyHill update at:" << t.getTime() << "ms" << endl;
    Logger::theLogger() << "Erasing invalid enemyHills" << endl;
    for(vector<Location>::iterator it=enemyHillsCache.begin();it!=enemyHillsCache.end();) {
        if ((grid[it->row][it->col].lastSeen==turn) && 
            (!grid[it->row][it->col].isHill)) 
            it = enemyHillsCache.erase(it);
        else 
            it++;
    }
    Logger::theLogger() <<"Finished enemyHill update at:" << t.getTime() << "ms" << endl;
    //update antMoveHistory

    for (movehistory_t::iterator it = antMoveHistory.begin();
	 it!=antMoveHistory.end();it++) {
	if (find(ants.begin(),ants.end(),it->first)==ants.end()) {
	    antMoveHistory.erase(it->first);
	}
    }

    //add any new ants.
    for (int i=0;i<ants.size();i++) {
	if (antMoveHistory.find(ants[i])==antMoveHistory.end()) {
	    map<Location,int> antHistory;
	    antHistory[ants[i]] = turn;
	    antMoveHistory[ants[i]]= antHistory;
	}
    }
    
    knownLandPercent = (double)visiblelocations/(rows*cols);
    //
}

bool State::isEdgeLocation(const int& row,const int& col) const {
    //unseen locations are not edge locations and we don't route to unseen
    if ((grid[row][col].lastSeen==0) ||
        (grid[row][col].lastSeen!=turn) ||
        (grid[row][col].isWater))
	return false;
    for(int d=0; d<TDIRECTIONS; d++)
    {
	Location loc = getLocation(Location(row,col), d);
	if (grid[loc.row][loc.col].lastSeen!=turn) 
	    return true;
    }
    return false;
}

bool State::findUntrackedFood(Location& foodLoc) {
    for(int i=0;i<food.size();i++) {
	if (grid[food[i].row][food[i].col].tracking==0) {
	    foodLoc = food[i];
	    return true;
	}
    }
    return false;
}

void State::findFriendlyAntsInRadius(const Location& loc,const double& radius,vector<Location>& antList) const {
    int offset = round(radius);

    Location start,end;    
    start.row = loc.row-offset;
    start.col = loc.col-offset;
    end.row = loc.row+offset;
    end.col = loc.col+offset;

    int i = start.row;

    while (i<=end.row) {
	int j = start.col;
	while(j<=end.col) {
	    //translate i/j from world coordinates to map coordinates
	    //and lookup the grid, 
	    //TODO: if the radius is greater than rows/2 and cols/2
	    //it might be more efficient to search the whole grid rather than
	    //use this loop. it will also cause ants to be added twice right now.
	    Location tLoc(mapX(i),mapY(j));
	    //Logger::theLogger() << tLoc <<":" << grid[tLoc.row][tLoc.col].ant << endl;

	    if ((grid[tLoc.row][tLoc.col].ant==0) && 
		(distance(tLoc,loc) <=radius)) {
		antList.push_back(tLoc);
	    }
	    j++;
	}
	i++;
    }
}

bool State::findEnemyHillsInRadius(const Location& loc,const double& radius,vector<Location>& hillLocs) const {
    for (int i=0;i<enemyHills.size();i++) {
	if (distance(loc,enemyHills[i]) < radius) {
	    hillLocs.push_back(enemyHills[i]);
	    return true;
	}
    }
    return false;
}

bool State::myHillInView(const Location& loc,const double& radius,Location& hill) const {
    for (int i=0;i<myHills.size();i++) {
	if (distance(loc,myHills[i]) < radius) {
	    hill = myHills[i];
	    return true;
	}
    }
    return false;
//     int offset = round(radius);

//     Location start,end;    
//     start.row = loc.row-offset;
//     start.col = loc.col-offset;
//     end.row = loc.row+offset;
//     end.col = loc.col+offset;

//     int i = start.row;

//     while (i<=end.row) {
// 	int j = start.col;
// 	while(j<=end.col) {
// 	    //translate i/j from world coordinates to map coordinates
// 	    //and lookup the grid, 
// 	    //TODO: if the radius is greater than rows/2 and cols/2
// 	    //it might be more efficient to search the whole grid rather than
// 	    //use this loop. it will also cause ants to be added twice right now.
// 	    Location tLoc(mapX(i),mapY(j));
// 	    //Logger::theLogger() << tLoc <<":" << grid[tLoc.row][tLoc.col].ant << endl;

// 	    if (((grid[tLoc.row][tLoc.col].isHill) && (grid[tLoc.row][tLoc.col].hillPlayer==0)) && 
// 		(distance(tLoc,loc) <=radius)) {
// 		return true;
// 	    }
// 	    j++;
// 	}
// 	i++;
//     }
//     return false;
}

void State::findEnemyAntsInRadius(const Location& loc,const double& radius,vector<Location>& antList) const {
    int offset = round(radius);

    Location start,end;    
    start.row = loc.row-offset;
    start.col = loc.col-offset;
    end.row = loc.row+offset;
    end.col = loc.col+offset;

    int i = start.row;

    while (i<=end.row) {
	int j = start.col;
	while(j<=end.col) {
	    //translate i/j from world coordinates to map coordinates
	    //and lookup the grid, 
	    //TODO: if the radius is greater than rows/2 and cols/2
	    //it might be more efficient to search the whole grid rather than
	    //use this loop. it will also cause ants to be added twice right now.
	    Location tLoc(mapX(i),mapY(j));
	    //Logger::theLogger() << tLoc <<":" << grid[tLoc.row][tLoc.col].ant << endl;

	    if ((grid[tLoc.row][tLoc.col].ant>0) && 
		(distance(tLoc,loc) <=radius)) {
		antList.push_back(tLoc);
	    }
	    j++;
	}
	i++;
    }
}

void State::findThisAntsEnemiesInRadius(const Location& loc,const double& radius,vector<Location>& antList,int player_id) const {
    int offset = round(radius);

    Location start,end;    
    start.row = loc.row-offset;
    start.col = loc.col-offset;
    end.row = loc.row+offset;
    end.col = loc.col+offset;

    int i = start.row;

    while (i<=end.row) {
	int j = start.col;
	while(j!=end.col) {
	    //translate i/j from world coordinates to map coordinates
	    //and lookup the grid, 
	    //TODO: if the radius is greater than rows/2 and cols/2
	    //it might be more efficient to search the whole grid rather than
	    //use this loop. it will also cause ants to be added twice right now.
	    Location tLoc(mapX(i),mapY(j));
	    //Logger::theLogger() << tLoc <<":" << grid[tLoc.row][tLoc.col].ant << endl;

	    if ((grid[tLoc.row][tLoc.col].ant>=0) && 
		(grid[tLoc.row][tLoc.col].ant!=player_id) &&
		(distance(tLoc,loc) <=radius)
		) {
		antList.push_back(tLoc);
	    }
	    j++;
	}
	i++;
    }
}

void State::findAntsInRadius(const Location& loc,const double& radius,vector<Location>& antList) const {
    int offset = round(radius);

    Location start,end;    
    start.row = loc.row-offset;
    start.col = loc.col-offset;
    end.row = loc.row+offset;
    end.col = loc.col+offset;

    int i = start.row;

    while (i<=end.row) {
	int j = start.col;
	while(j<=end.col) {
	    //translate i/j from world coordinates to map coordinates
	    //and lookup the grid, 
	    //TODO: if the radius is greater than rows/2 and cols/2
	    //it might be more efficient to search the whole grid rather than
	    //use this loop. it will also cause ants to be added twice right now.
	    Location tLoc(mapX(i),mapY(j));
	    //Logger::theLogger() << tLoc <<":" << grid[tLoc.row][tLoc.col].ant << endl;

	    if ((grid[tLoc.row][tLoc.col].ant>=0) && 
		(distance(tLoc,loc) <=radius)) {
		antList.push_back(tLoc);
	    }
	    j++;
	}
	i++;
    }
}

//multidimensional array of visited nodes
void State::extractPath(const Location& startLoc,const Location& endLoc,const vector< vector<int> >& visited,stack<int>& path) const{
    Logger::theLogger() << "Extracting path from : " << startLoc << " to " << endLoc << endl;
    Location curLoc  = startLoc;

    while (visited[curLoc.row][curLoc.col]!=1){
//	Logger::theLogger() << "curLoc is:" << curLoc <<" with visited:" << visited[curLoc.row][curLoc.col] << endl;
	double min_man_dist = manhattan_distance(curLoc,endLoc);
	double min_dist = LARGEVAL+0.1;
	int direction;
	Location nextLoc;
	int min_visited=visited[curLoc.row][curLoc.col];
	bool first =true;
	for(int d=0;d<TDIRECTIONS;d++) {
	    Location nLoc = getLocation(curLoc,d);
	    //Logger::theLogger() << "nLoc is:" << nLoc <<" with visited:" << visited[nLoc.row][nLoc.col] << endl;
	    //Logger::theLogger() << "min_dist is: " << min_dist << " nLoc->endLoc is:" << distance(nLoc,endLoc) << endl;
	    //Logger::theLogger() << "min_man_dist is:" << min_man_dist << " nLoc->endLoc is:" << manhattan_distance(nLoc,endLoc) << endl;
	    if ((visited[nLoc.row][nLoc.col]!=0) &&
		(visited[nLoc.row][nLoc.col] < visited[curLoc.row][curLoc.col])
		) 
	    {
		//Logger::theLogger() << " found next step " << endl;
		if (first) {
		    first = false;
		    min_visited = visited[nLoc.row][nLoc.col];
		    min_dist = distance(nLoc,endLoc);
		    min_man_dist = manhattan_distance(nLoc,endLoc);
		    direction = d;
		    nextLoc = nLoc;
		}
		else if (distance(nLoc,endLoc) < min_dist) {
		    min_visited = visited[nLoc.row][nLoc.col];
		    min_dist = distance(nLoc,endLoc);
		    min_man_dist = manhattan_distance(nLoc,endLoc);
		    direction = d;
		    nextLoc = nLoc;
		}			
	    }
	}
	//Logger::theLogger()  << "nextLoc is:" << nextLoc <<" with visited:" << visited[nextLoc.row][nextLoc.col]<< endl;
	//we need to index with a trick to reverse directions
	path.push((direction+2)%4);
	curLoc = nextLoc;
    }
}


int State::extractMove(const Location& ant,const vector< vector<int> >& visited) const {
    int min_val= LARGEVAL;
    int min_d = -1;
    for (int i=0;i<TDIRECTIONS;i++) {
        Location nLoc = getLocation(ant,i);
        if (visited[nLoc.row][nLoc.col] && (visited[nLoc.row][nLoc.col] <min_val)) {
            min_val = visited[nLoc.row][nLoc.col];
            min_d = i;
        }
    }
    return min_d;
}

void State::multiSourceBFSFindMove(const vector<Location>& ants,
                                   map<Location,int>& antMoves) const {
    vector< vector<int> > visited(rows,vector<int>(cols,0)) ;
    int depth = 1;
    int numNodesExplored = 0;

    //add hills as potential targets
    queue<Location> openQueue;

    for(vector<Location>::const_iterator it=enemyHills.begin();it!=enemyHills.end();
        it++) {
        openQueue.push(*it);
        visited[it->row][it->col] = depth;
    }
    for(vector<Location>::const_iterator it=enemyHillsCache.begin();it!=enemyHillsCache.end();it++) {
        if (std::find(enemyHills.begin(),enemyHills.end(),*it)==enemyHills.end()) {
            openQueue.push(*it);
            visited[it->row][it->col]=depth;
        }
    }
    //only add edge locations if we don't have an enemy hill in view
    if (openQueue.empty()) {
        for(int i=0;i<rows;i++) {
            for (int j=0;j<cols;j++) {
                if (isEdgeLocation(i,j)) {
                    openQueue.push(Location(i,j));
                    visited[i][j] = depth;
                }
            }
        }
    }
    while (!openQueue.empty()) {
        numNodesExplored++;
        Location curLoc = openQueue.front();
        openQueue.pop();
        depth+=1;
        for (int d=0;d<TDIRECTIONS;d++) {
            Location nLoc = getLocation(curLoc,d);
            if ((visited[nLoc.row][nLoc.col]==0) &&
                !grid[nLoc.row][nLoc.col].isWater &&
                !grid[nLoc.row][nLoc.col].isHill &&
                (grid[nLoc.row][nLoc.col].lastSeen!=0)) {
                visited[nLoc.row][nLoc.col] = visited[curLoc.row][curLoc.col]+1;
                openQueue.push(nLoc);
            }
        }
    }
    for(vector<Location>::const_iterator it=ants.begin();
        it!=ants.end();
        it++) {
        int d = extractMove(*it,visited);
        Logger::theLogger() << "Extracted move for ant:" << *it << ":" << d << endl;

        if (d>=0) {
            Logger::theLogger() << "Extracted move for ant:" << *it << ":" << CDIRECTIONS[d] << endl;
            antMoves[*it] = d;
        }
    }
    for (int i=0;i<rows;i++)  {
        for (int j=0;j<cols;j++) {
            if (visited[i][j]==1) {
                Logger::theLogger() << setw(1) << "*";
            }
            else if (std::find(ants.begin(),ants.end(),Location(i,j))!=ants.end()) {
                Logger::theLogger() << setw(1) << "#";
            }
            else if (visited[i][j]!=0) {    
                Logger::theLogger() << setw(1) << "x" ; //visited[i][j];
            }
            else {
                Logger::theLogger() << setw(1) << "0" ;
            }
        }
        Logger::theLogger() <<endl;
    }

}

struct PathNode {
    float h;
    Location loc;
    
    PathNode(float hVal,Location l):h(hVal),loc(l) {}
    bool operator < (const PathNode& other) const {
	return (h>other.h);
    }
};

//finds the shortest path from startLoc to endLoc
bool State::findPath(const Location& startLoc,const Location& endLoc, 
		     stack<int>& path,int& numNodesExplored) const {
    priority_queue<PathNode> openQueue;
    openQueue.push(PathNode(distance(startLoc,endLoc),startLoc));
    
    map<Location,bool> closedSet;
    vector< vector<int> > visited(rows,vector<int>(cols,0));
    //all nodes
    int depth = 1;
    numNodesExplored = 0;
    bool found=false;
    visited[startLoc.row][startLoc.col] = depth;
    while (!openQueue.empty()) {
	numNodesExplored++;
	PathNode curNode = openQueue.top();
	openQueue.pop();
	Location curLoc = curNode.loc;
	closedSet[curLoc] =true;
	if (curLoc.row == endLoc.row && curLoc.col == endLoc.col) {
	    found = true;
	    break;
	}
	else {
	    //Search didn't terminate so explore adjoining nodes
	    depth+=1;//increase depth of search tree by 1
	    for (int d=0;d<TDIRECTIONS;d++) {
		Location nLoc = getLocation(curLoc,d);
		if (closedSet.find(nLoc)!=closedSet.end())  {
		    continue;
		}
		if (nLoc==endLoc)  {
		    visited[nLoc.row][nLoc.col]  = visited[curLoc.row][curLoc.col]+1;
		    found=true;
		    break;
		}
		if (!grid[nLoc.row][nLoc.col].isWater && //don't route over water
		    (grid[nLoc.row][nLoc.col].lastSeen!=0) &&
		    (grid[nLoc.row][nLoc.col].ant==-1))// &&
		    //!grid[nLoc.row][nLoc.col].isHill)
		    {
			//if the node has not yet been visited
			//and is not water and has been seen before
			//then explore this node
		    
			if (visited[nLoc.row][nLoc.col]==0) {
			    visited[nLoc.row][nLoc.col] = visited[curLoc.row][curLoc.col]+1;
			    openQueue.push(PathNode(visited[nLoc.row][nLoc.col]+distance(nLoc,endLoc),nLoc));
			}
			else if (visited[nLoc.row][nLoc.col]>(visited[curLoc.row][curLoc.col]+1)) {
			    visited[nLoc.row][nLoc.col] = visited[curLoc.row][curLoc.col]+1;
			}
		    }
	    }
	    if (found) 
		break;
	}
    }
    //if we found an ant extract the Path
    //and return the ant location and the path from the ant to the location
    if (found) {
	extractPath(endLoc,startLoc,visited,path);
	if (path.empty()) {
	    for (int i=0;i<rows;i++)  {
		for (int j=0;j<cols;j++) {
		    if (visited[i][j]==1) {
			Logger::theLogger() << setw(3) << "*";
		    }
		    else if ((i==endLoc.row) && (j==endLoc.col)) {
			Logger::theLogger() << setw(3)<< "#";
		    }
		    else if (visited[i][j]!=0) {    
			Logger::theLogger() << setw(3) << visited[i][j];
		    }
		    else {
			Logger::theLogger() << setw(3) << "0" ;
		    }
		}
		Logger::theLogger() <<endl;
	    }
	    return false;
	}
	return true;
    }
    else {
	Logger::theLogger() << "Path finding failed for "  << startLoc << " to " << endLoc << endl;
	for (int i=0;i<rows;i++)  {
	    for (int j=0;j<cols;j++) {
		if (visited[i][j]==1) {
		    Logger::theLogger() << setw(3) << "*";
		}
		else if ((i==endLoc.row) && (j==endLoc.col)) {
		    Logger::theLogger() << setw(3)<< "#";
		}
		else if (visited[i][j]!=0) {    
		    Logger::theLogger() << setw(3) << visited[i][j];
		}
		else {
		    Logger::theLogger() << setw(3) << "0" ;
		}
	    }
	    Logger::theLogger() <<endl;
	}
	return false;
    }
}

//Returns True if a food was found
//False otherwise
//if found the second parameter is the location of the food and
//path parameter contains the full path from loc to foodLoc
bool State::findClosestFood(const Location& loc,Location& foodLoc,stack<int>& path,int& numNodesExplored) const{
  //Crappy implementation, do a BFS starting at loc till we hit a friendly ant
  //at that point terminate the ant and send it along the reverse path 
    queue< Location > openQueue;
    openQueue.push(loc);

    //We use visited to track the path and extract it once we find the target
    vector < vector<int> >  visited(rows,vector<int>(cols,0));
    //we search around the location till we hit an ant or we exhaust 
    //all nodes
    int depth = 1;
    numNodesExplored = 0;
    Location finalLoc;
    bool found=false;
    visited[loc.row][loc.col] = depth;

    while (!openQueue.empty()) {
	numNodesExplored++;
	Location curLoc = openQueue.front();
	openQueue.pop();
	if (grid[curLoc.row][curLoc.col].isFood && 
	    (grid[curLoc.row][curLoc.col].ant!=0)) //&&
	    //&& 
	    //grid[curLoc.row][curLoc.col].tracking==0)
	{
	    finalLoc = curLoc;
	    found = true;
	    break;
	}
	else {
	    //Search didn't terminate so explore adjoining nodes
	    depth+=1;//increase depth of search tree by 1
	    for (int d=0;d<TDIRECTIONS;d++) {
		Location nLoc = getLocation(curLoc,d);
		if ((visited[nLoc.row][nLoc.col]==0) &&// explore only unvisited nodes 
		    !grid[nLoc.row][nLoc.col].isWater && //don't route over water
		    !grid[nLoc.row][nLoc.col].isHill && //don't route over hills
		    (grid[nLoc.row][nLoc.col].lastSeen!=0)  &&
		    (distance(loc,nLoc)<=viewradius*2)
// 		    && //don't try to explore nodes never seen
// 		    (grid[nLoc.row][nLoc.col].ant!=0)//don't try to explore routes via nodes with our ants
		    ) {
		    //if the node has not yet been visited
		    //and is not water and has been seen before
		    //then explore this node
		    visited[nLoc.row][nLoc.col] = visited[curLoc.row][curLoc.col]+1;
		    openQueue.push(nLoc);
		}
	    }
	}
    }
    //if we found an ant extract the Path
    //and return the ant location and the path from the ant to the location
    if (found) {
	extractPath(finalLoc,loc,visited,path);
	foodLoc = finalLoc;
	return true;
    }
    else 
	return false;
}

//Returns True if an enemyAnt was found
//False otherwise
//if found the second parameter is the location of the enemyAnt and
//path parameter contains the full path from loc to enemyAnt
bool State::findClosestEnemyAnt(const Location& loc,Location& antLoc,stack<int>& path,int& numNodesExplored) const {
  //Crappy implementation, do a BFS starting at loc till we hit a friendly ant
  //at that point terminate the ant and send it along the reverse path 
    queue< Location > openQueue;
    openQueue.push(loc);
    

    if (playerQ.empty())
      return false;
    int weakestPlayer=playerQ.top().playerId;

    Logger::theLogger() << "Weakest Player is:" << weakestPlayer << endl;
    //We use visited to track the path and extract it once we find the target
    vector < vector<int> >  visited(rows,vector<int>(cols,0));
    //we search around the location till we hit an ant or we exhaust 
    //all nodes
    int depth = 1;
    numNodesExplored = 0;
    Location finalLoc;
    bool found=false;
    visited[loc.row][loc.col] = depth;

    while (!openQueue.empty()) {
	numNodesExplored++;
	Location curLoc = openQueue.front();
	openQueue.pop();
 	if (grid[curLoc.row][curLoc.col].ant>0){
	    finalLoc = curLoc;
	    found = true;
	    break;
	}
	else {
	    //Search didn't terminate so explore adjoining nodes
	    depth+=1;//increase depth of search tree by 1
	    for (int d=0;d<TDIRECTIONS;d++) {
		Location nLoc = getLocation(curLoc,d);
		if (grid[nLoc.row][nLoc.col].ant>0) {
		    visited[nLoc.row][nLoc.col] = visited[curLoc.row][curLoc.col]+1;
		    finalLoc = nLoc;
		    found = true;
		    break;
		}

		if ((visited[nLoc.row][nLoc.col]==0) &&
		    !grid[nLoc.row][nLoc.col].isWater &&
		    !grid[nLoc.row][nLoc.col].isHill && 
		    (grid[nLoc.row][nLoc.col].lastSeen!=0) &&
		    (distance(loc,nLoc)<=viewradius)
		    ) {
		    //if the node has not yet been visited
		    //and is not water and has been seen before
		    //then explore this node
		    visited[nLoc.row][nLoc.col] = visited[curLoc.row][curLoc.col]+1;
		    openQueue.push(nLoc);
		}
	    }
	}
	if (found) 
	    break;
    }
    //if we found an ant extract the Path
    //and return the ant location and the path from the ant to the location
    if (found) {
	extractPath(finalLoc,loc,visited,path);
	antLoc = finalLoc;
	return true;
    }
    else 
	return false;
}

bool State::findClosestEnemyHill2(const Location& loc,Location& hillLoc,stack<int>& path, int& numNodesExplored) const {
    int maxPathLen=LARGEVAL;
    for(int i=0;i<enemyHills.size();i++) {
	if (distance(enemyHills[i],loc) > viewradius*2) 
	    continue;
	stack<int> pathToHill;
	int numNodes;
	if (findPath(loc,enemyHills[i],pathToHill,numNodes)) { 
	    if (pathToHill.size() < maxPathLen) {
		path=pathToHill;
		numNodesExplored=numNodes;
		maxPathLen=pathToHill.size();
		hillLoc=enemyHills[i];
	    }
	}
    }
    if (maxPathLen==LARGEVAL) 
	return false;

    return true;
}

//Returns True if an enemyHill was found
//False otherwise
//if found the second parameter is the location of the enemyHill and
//path parameter contains the full path from loc to enemyHill
bool State::findClosestEnemyHill(const Location& loc,Location& hillLoc,stack<int>& path,int& numNodesExplored) const {
  //Crappy implementation, do a BFS starting at loc till we hit a enemy hill
  //at that point terminate the ant and send it along the reverse path 
    queue< Location > openQueue;
    openQueue.push(loc);
    

    //We use visited to track the path and extract it once we find the target
    vector < vector<int> >  visited(rows,vector<int>(cols,0));
    //we search around the location till we hit an ant or we exhaust 
    //all nodes
    int depth = 1;
    numNodesExplored = 0;
    Location finalLoc;
    bool found=false;
    visited[loc.row][loc.col] = depth;

    while (!openQueue.empty()) {
	numNodesExplored++;
	Location curLoc = openQueue.front();
	openQueue.pop();
 	if (grid[curLoc.row][curLoc.col].isHill && (grid[curLoc.row][curLoc.col].hillPlayer>0)) {
	    finalLoc = curLoc;
	    found = true;
	    break;
	}
	else {
	    //Search didn't terminate so explore adjoining nodes
	    depth+=1;//increase depth of search tree by 1
	    for (int d=0;d<TDIRECTIONS;d++) {
		Location nLoc = getLocation(curLoc,d);
		if (grid[nLoc.row][nLoc.col].isHill && (grid[curLoc.row][curLoc.col].hillPlayer>0)) {
		    visited[nLoc.row][nLoc.col] = visited[curLoc.row][curLoc.col]+1;
		    finalLoc = nLoc;
		    found = true;
		    break;
		}

		if ((visited[nLoc.row][nLoc.col]==0) &&
		    !grid[nLoc.row][nLoc.col].isWater &&
		    (grid[nLoc.row][nLoc.col].lastSeen!=0) &&
		    (distance(loc,nLoc)<=viewradius)
		    ) {
		    //if the node has not yet been visited
		    //and is not water and has been seen before
		    //then explore this node
		    visited[nLoc.row][nLoc.col] = visited[curLoc.row][curLoc.col]+1;
		    openQueue.push(nLoc);
		}
	    }
	}
	if (found) 
	    break;
    }
    //if we found an ant extract the Path
    //and return the ant location and the path from the ant to the location
    if (found) {
	extractPath(finalLoc,loc,visited,path);
	hillLoc = finalLoc;
	return true;
    }
    else 
	return false;
}

//Returns True if an friendlyAnt was found
//False otherwise
//if found the second parameter is the location of the enemyAnt and
//path parameter contains the full path from loc to enemyAnt
bool State::findClosestFriendlyAnt(const Location& loc,Location& antLoc,stack<int>& path,int& numNodesExplored) const {
  //Crappy implementation, do a BFS starting at loc till we hit a friendly ant
  //at that point terminate the ant and send it along the reverse path 
    queue< Location > openQueue;
    openQueue.push(loc);

    //We use visited to track the path and extract it once we find the target
    vector < vector<int> >  visited(rows,vector<int>(cols,0));
    //we search around the location till we hit an ant or we exhaust 
    //all nodes
    int depth = 1;
    numNodesExplored = 0;
    Location finalLoc;
    bool found=false;
    visited[loc.row][loc.col] = depth;

    while (!openQueue.empty()) {
	numNodesExplored++;
	Location curLoc = openQueue.front();
	openQueue.pop();
	//we don't check first location for an ant as we are usually
	//trying to find friendly ants close to another one.
	if (grid[curLoc.row][curLoc.col].ant==0 and curLoc!=loc) {
	    finalLoc = curLoc;
	    found = true;
	    break;
	}
	else {
	    //Search didn't terminate so explore adjoining nodes
	    depth+=1;//increase depth of search tree by 1
	    for (int d=0;d<TDIRECTIONS;d++) {
		Location nLoc = getLocation(curLoc,d);
		if ((visited[nLoc.row][nLoc.col]==0) &&
		    !grid[nLoc.row][nLoc.col].isWater &&
		    !grid[nLoc.row][nLoc.col].isHill  &&
		    (grid[nLoc.row][nLoc.col].lastSeen!=0) &&
		    (distance(loc,nLoc)<=viewradius*2)
		    ) {
		    //if the node has not yet been visited
		    //and is not water and has been seen before
		    //then explore this node
		    visited[nLoc.row][nLoc.col] = visited[curLoc.row][curLoc.col]+1;
		    openQueue.push(nLoc);
		}
	    }
	}
    }
    //if we found an ant extract the Path
    //and return the ant location and the path from the ant to the location
    if (found) {
	extractPath(finalLoc,loc,visited,path);
	antLoc = finalLoc;
	return true;
    }
    else 
	return false;
}

void State::findFoodInRadius(const Location& loc, double radius,vector<Location>& foodList) const {
    //find friendly ant locations
    for(int i =0;i<food.size();i++) {
      if (distance(loc,food[i]) <= (radius+0.1)) {
	    foodList.push_back(food[i]);
	}
    }
}

//Each location is mapped to a 4x4 square area
bool State::inSameOrAdjacentQuadrant(const Location& loc1,const Location& loc2) {
  int l1qr = loc1.row%4;
  int l1qc = loc1.col%4;
  int l2qr = loc2.row%4;
  int l2qc = loc2.col%4;

  if (manhattan_distance(Location(l1qr,l1qc),Location(l2qr,l2qc)) <=1) 
      return true;
  return false;
}

double State::calcLocPriority(const Location& loc) {
    double priority=grid[loc.col][loc.row].lastSeen+1;
    if (grid[loc.row][loc.col].deadAnts.size()>0) 
	priority-=30;

    if (grid[loc.row][loc.col].ant>0) 
	priority-=30;//automatically bump down locations where enemy ants are located, we want our ants to expand away from enemies
    vector<Location> antList;
    findEnemyAntsInRadius(loc,attackradius,antList);
    if (antList.size()>0)
	priority -=20;//squares with nearby enemy ants bump them down in priority too.
    vector<Location> foodList;
    findFoodInRadius(loc,attackradius*3,foodList);
    if (foodList.size()>0) 
	priority -=300; //bump up squares with nearby food higher in priority
    return priority;
}

/*
    This is the output function for a state. It will add a char map
    representation of the state to the output stream passed to it.

    For example, you might call "cout << state << endl;"
*/
ostream& operator<<(ostream &os, const State &state)
{
    //print some stats first
    os << "ants:" << state.ants.size() << endl;
    os << "enemies:" << state.enemies.size() << endl;
    os << "food:"  << state.food.size() << endl;
    for(int row=0; row<state.rows; row++)
    {
        for(int col=0; col<state.cols; col++)
        {
            if(state.grid[row][col].isWater)
                os << '%';
            else if(state.grid[row][col].isFood)
                os << '*';
            else if(state.grid[row][col].isHill)
                os << (char)('A' + state.grid[row][col].hillPlayer);
            else if(state.grid[row][col].ant >= 0)
                os << (char)('a' + state.grid[row][col].ant);
            else if(state.grid[row][col].lastSeen == 0)
                os << '?';
            else if(state.grid[row][col].lastSeen < state.turn)
                os << '~';
            else
                os << '.';
        }
        os << endl;
    }

    return os;
};

//input function
istream& operator>>(istream &is, State &state)
{
    int row, col, player;
    string inputType, junk;

    //finds out which turn it is
    while(is >> inputType)
    {
        if(inputType == "end")
        {
            state.gameover = 1;
            break;
        }
        else if(inputType == "turn")
        {
            is >> state.turn;
            break;
        }
        else //unknown line
            getline(is, junk);
    }

    if(state.turn == 0)
    {
        //reads game parameters
        while(is >> inputType)
        {
            if(inputType == "loadtime")
                is >> state.loadtime;
            else if(inputType == "turntime")
                is >> state.turntime;
            else if(inputType == "rows")
                is >> state.rows;
            else if(inputType == "cols")
                is >> state.cols;
            else if(inputType == "turns")
                is >> state.turns;
	    else if (inputType == "player_seed") {
		is >> state.seed;
                srand(state.seed);
            }
            else if(inputType == "viewradius2")
            {
                is >> state.viewradius;
                state.viewradius = sqrt(state.viewradius);
            }
            else if(inputType == "attackradius2")
            {
                is >> state.attackradius;
                state.attackradius = sqrt(state.attackradius);
            }
            else if(inputType == "spawnradius2")
            {
                is >> state.spawnradius;
                state.spawnradius = sqrt(state.spawnradius);
            }
            else if(inputType == "ready") //end of parameter input
            {
                state.timer.start();
                break;
            }
            else    //unknown line
                getline(is, junk);
        }
    }
    else
    {
        //reads information about the current turn
        while(is >> inputType)
        {
            if(inputType == "w") //water square
            {
                is >> row >> col;
                state.grid[row][col].isWater = 1;
            }
            else if(inputType == "f") //food square
            {
                is >> row >> col;
                state.grid[row][col].isFood = 1;
                state.food.push_back(Location(row, col));
            }
            else if(inputType == "a") //live ant square
            {
                is >> row >> col >> player;
                state.grid[row][col].ant = player;
                if(player == 0)
                    state.ants.push_back(Location(row, col));
                else
                    state.enemies.push_back(Location(row, col));
            }
            else if(inputType == "d") //dead ant square
            {
                is >> row >> col >> player;
                state.grid[row][col].deadAnts.push_back(player);
            }
            else if(inputType == "h")
            {
                is >> row >> col >> player;
                state.grid[row][col].isHill = 1;
                state.grid[row][col].hillPlayer = player;
                if(player == 0)
                    state.myHills.push_back(Location(row, col));
                else {
                    state.enemyHills.push_back(Location(row, col));
                    if (std::find(state.enemyHillsCache.begin(),state.enemyHillsCache.end(),
                             Location(row,col))==state.enemyHillsCache.end()) {
                        state.enemyHillsCache.push_back(Location(row,col));
                    }
                }
            }
            else if(inputType == "players") //player information
                is >> state.noPlayers;
            else if(inputType == "scores") //score information
            {
                state.scores = vector<double>(state.noPlayers, 0.0);
                for(int p=0; p<state.noPlayers; p++)
                    is >> state.scores[p];
            }
            else if(inputType == "go") //end of turn input
            {
                if(state.gameover)
                    is.setstate(std::ios::failbit);
                else
                    state.timer.start();
                break;
            }
            else //unknown line
                getline(is, junk);
        }
    }

    return is;
};
